#include <iostream>
#include <string>
// ROS
#include <image_transport/image_transport.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/utilities.hpp>
#include <camera_info_manager/camera_info_manager.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
// OpenCV
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>

namespace ros2_video
{
   class VideoNode : public rclcpp::Node
   { 
   public:
      explicit VideoNode(const rclcpp::NodeOptions &options) : Node("ros2_video_node", options)
      {
         RCLCPP_INFO(this->get_logger(), "Starting VideoNode!");

         //构建相机参数信息
         camera_name_ = this->declare_parameter("camera_name", "video_test");
         camera_info_manager_ =
            std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);
         auto camera_info_url =
            this->declare_parameter("camera_info_url", "package://ros2_video/config/camera_info.yaml");
         if (camera_info_manager_->validateURL(camera_info_url)) {
            camera_info_manager_->loadCameraInfo(camera_info_url);
            camera_info_msg_ = camera_info_manager_->getCameraInfo();
         } else {
            RCLCPP_WARN(this->get_logger(), "Invalid camera info URL: %s", camera_info_url.c_str());
         }

         // 获取视频路径
         auto video_path = this->declare_parameter("video_path", "");
         if (video_path.find("package://") == 0) {
            // 处理package://格式的路径
            auto package_name = video_path.substr(10, video_path.find("/", 10) - 10);
            auto package_path = ament_index_cpp::get_package_share_directory(package_name);
            auto relative_path = video_path.substr(video_path.find("/", 10) + 1);
            video_path = package_path + "/" + relative_path;
         }
         RCLCPP_INFO(this->get_logger(), "Opening video file: %s", video_path.c_str());
         video.open(video_path);
         if (!video.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to open video file: %s", video_path.c_str());
            return;
         }
         
         // 线程内使用匿名函数，可以在类中直接调用
         capture_thread_ = std::thread{[this]() -> void {

            RCLCPP_INFO(this->get_logger(), "Publishing image!");

            while (rclcpp::ok())
            {
               if (video.read(image))
               {
                  /* code */

                  cv_image.header.stamp = this->now();
                  cv_image.header.frame_id = "camera_optical_frame";
                  cv_image.encoding = "bgr8";
                  cv_image.image = image;

                  image_msg_ = *cv_image.toImageMsg();
                  video_pub_->publish(image_msg_);

                  camera_info_msg_.header = image_msg_.header;
                  camera_info_pub_->publish(camera_info_msg_);
                  cv::waitKey(1000/video.get(5));
               }
               else
               {
                  // 如果视频播放完毕，则重新开始播放
                  video.set(cv::CAP_PROP_POS_FRAMES, 0);
                  // break;
                  //  RCLCPP_INFO(this->get_logger(),"Waiting Image!");
                  // video.open("src/ros2_video/ood_blue.mp4");
               }
            }
         }};
         
      }

      ~VideoNode() override
      {
         if (capture_thread_.joinable()) {
            capture_thread_.join();
         }

         RCLCPP_INFO(this->get_logger(), "VideoNode destroyed!");
      }


   private:
      cv::VideoCapture video;
      cv::Mat image;
      rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr video_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/image_raw", 10);
      rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("/camera_info", 10);
      std::string camera_name_;
      std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
      sensor_msgs::msg::CameraInfo camera_info_msg_;
      sensor_msgs::msg::Image image_msg_;
      cv_bridge::CvImage cv_image;
      std::thread capture_thread_;

   };
}
#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(ros2_video::VideoNode)
